#include "ros/ros.h"
#include "serial/serial.h"
#include "std_msgs/String.h"
#include <std_msgs/Float32.h>
#include <tcr100/ImuData.h>
#include <tcr100/serial.h>
#include "std_msgs/Empty.h"
#include "tf/transform_broadcaster.h"

serial::Serial ser; //声明串口对象
#define BUFFERSIZE 33
unsigned char buff[BUFFERSIZE],counter;
unsigned char sign=0;
float angle,acc_x,v_th;

unsigned char check_yaw(unsigned char *buffer)
{
  unsigned char ret,sum;
for(int k=0;k<BUFFERSIZE;k++)
{
  if(buffer[k]==0x55&&buffer[k+1]==0x53)
  {
      sum+=buffer[k]+buffer[k+1]+buffer[k+2]+buffer[k+3]+buffer[k+4]+buffer[k+5]+buffer[k+6]+buffer[k+7]+buffer[k+8]+buffer[k+9];
      if(sum==buffer[k+10])
      {
          ret=1;
          angle=(short(buffer[k+7]<<8| buffer[k+6]))/32768.0*180;
          printf("angle=:%f\n",angle);
      }
      else
      {
          ret=0;
      }
  }
}
  return ret;
}
/***************************************************************************/

unsigned char check_acc(unsigned char *buffer)
{
  unsigned char ret,sum;
for(int j=0;j<BUFFERSIZE;j++)
{
  if(buffer[j]==0x55&&buffer[j+1]==0x51)
  {
      sum+=buffer[j]+buffer[j+1]+buffer[j+2]+buffer[j+3]+buffer[j+4]+buffer[j+5]+buffer[j+6]+buffer[j+7]+buffer[j+8]+buffer[j+9];
      if(sum==buffer[j+10])
      {
          ret=1;
          acc_x=(short(buffer[j+3]<<8| buffer[j+2]))/32768.0*2000;
          printf("acc= %f\n",acc_x);
      }
      else
      {
          ret=0;
      }
  }
}
  return ret;
}
int main (int argc, char** argv){
    ros::init(argc, argv, "serial_example_node");
    ros::NodeHandle nh;
    ros::Publisher yaw_pub = nh.advertise<tcr100::ImuData>("yaw_data", 1000);

    try
    {
        ser.setPort("/dev/ttyS1");
        ser.setBaudrate(9600);
        serial::Timeout to = serial::Timeout::simpleTimeout(1000);
        ser.setTimeout(to);
        ser.open();
    }
    catch (serial::IOException& e)
    {
        ROS_ERROR_STREAM("Unable to open port ");
        return -1;
    }
    if(ser.isOpen()){
        ROS_INFO_STREAM("Serial Port initialized");
    }else{
        return -1;
    }
    ros::Rate loop_rate(50);
    while(ros::ok()){
        tcr100::serial result;
        ros::spinOnce();

        if(ser.available()){
            //ROS_INFO_STREAM("Reading from serial port");
            ser.read(buff,BUFFERSIZE);
            if(check_yaw(buff)!=0)
            {
                tcr100::ImuData msg;
                msg.yaw=angle;
                yaw_pub.publish(msg);
               // ROS_INFO_STREAM("End reading from serial port");
            }
            memset(buff,0,BUFFERSIZE);
        }
        loop_rate.sleep();
    }
}
